﻿#include "algorithmvision3d.h"
#include <vector>
#include <algorithm>


#define EPSILON 0.000001
#define PI 3.14159265358979323846264338

using namespace std;
//using namespace cv;
using namespace Eigen;

AlgorithmVision3D::AlgorithmVision3D()
{
}

//创建一个Eigen::Matrix4f矩阵
Eigen::Matrix4f AlgorithmVision3D::CreateMatrix4f(
	const float& fm00, const float& fm01, const float& fm02, const float& fm03,
	const float& fm10, const float& fm11, const float& fm12, const float& fm13,
	const float& fm20, const float& fm21, const float& fm22, const float& fm23,
	const float& fm30, const float& fm31, const float& fm32, const float& fm33)
{
	Eigen::Matrix4f fMatrix = Eigen::Matrix4f::Identity();
	fMatrix(0, 0) = fm00; fMatrix(0, 1) = fm01; fMatrix(0, 2) = fm02; fMatrix(0, 3) = fm03;
	fMatrix(1, 0) = fm10; fMatrix(1, 1) = fm11; fMatrix(1, 2) = fm12; fMatrix(1, 3) = fm13;
	fMatrix(2, 0) = fm20; fMatrix(2, 1) = fm21; fMatrix(2, 2) = fm22; fMatrix(2, 3) = fm23;
	fMatrix(3, 0) = fm30; fMatrix(3, 1) = fm31; fMatrix(3, 2) = fm32; fMatrix(3, 3) = fm33;
	return fMatrix;
}
//创建一个Eigen::Matrix4d矩阵
Eigen::Matrix4d AlgorithmVision3D::CreateMatrix4d(
	const float& dm00, const float& dm01, const float& dm02, const float& dm03,
	const float& dm10, const float& dm11, const float& dm12, const float& dm13,
	const float& dm20, const float& dm21, const float& dm22, const float& dm23,
	const float& dm30, const float& dm31, const float& dm32, const float& dm33)
{
	Eigen::Matrix4d dMatrix = Eigen::Matrix4d::Identity();
	dMatrix(0, 0) = dm00; dMatrix(0, 1) = dm01; dMatrix(0, 2) = dm02; dMatrix(0, 3) = dm03;
	dMatrix(1, 0) = dm10; dMatrix(1, 1) = dm11; dMatrix(1, 2) = dm12; dMatrix(1, 3) = dm13;
	dMatrix(2, 0) = dm20; dMatrix(2, 1) = dm21; dMatrix(2, 2) = dm22; dMatrix(2, 3) = dm23;
	dMatrix(3, 0) = dm30; dMatrix(3, 1) = dm31; dMatrix(3, 2) = dm32; dMatrix(3, 3) = dm33;
	return dMatrix;
}

//创建一个Eigen::Matrix3f矩阵
Eigen::Matrix3f AlgorithmVision3D::CreateMatrix3f(
	const float& fm00, const float& fm01, const float& fm02,
	const float& fm10, const float& fm11, const float& fm12,
	const float& fm20, const float& fm21, const float& fm22)
{
	Eigen::Matrix3f fMatrix = Eigen::Matrix3f::Identity();
	fMatrix(0, 0) = fm00; fMatrix(0, 1) = fm01; fMatrix(0, 2) = fm02;
	fMatrix(1, 0) = fm10; fMatrix(1, 1) = fm11; fMatrix(1, 2) = fm12;
	fMatrix(2, 0) = fm20; fMatrix(2, 1) = fm21; fMatrix(2, 2) = fm22;
	return fMatrix;
}
//创建一个Eigen::Matrix3d矩阵
Eigen::Matrix3d AlgorithmVision3D::CreateMatrix3d(
	const float& dm00, const float& dm01, const float& dm02,
	const float& dm10, const float& dm11, const float& dm12,
	const float& dm20, const float& dm21, const float& dm22)
{
	Eigen::Matrix3d dMatrix = Eigen::Matrix3d::Identity();
	dMatrix(0, 0) = dm00; dMatrix(0, 1) = dm01; dMatrix(0, 2) = dm02;
	dMatrix(1, 0) = dm10; dMatrix(1, 1) = dm11; dMatrix(1, 2) = dm12;
	dMatrix(2, 0) = dm20; dMatrix(2, 1) = dm21; dMatrix(2, 2) = dm22;
	return dMatrix;
}

Eigen::Matrix4d AlgorithmVision3D::Matrix3d2Matrix4d(const Eigen::Matrix3d& Matrix3dR, const double& dTx, const double& dTy, const double& dTz)
{
	Eigen::Matrix4d Pose = CreateMatrix4d(
		Matrix3dR(0, 0), Matrix3dR(0, 1), Matrix3dR(0, 2), dTx,
		Matrix3dR(1, 0), Matrix3dR(1, 1), Matrix3dR(1, 2), dTy,
		Matrix3dR(2, 0), Matrix3dR(2, 1), Matrix3dR(2, 2), dTz);
	return Pose;
}
Eigen::Matrix4f AlgorithmVision3D::Matrix3f2Matrix4f(const Eigen::Matrix3f& Matrix3fR, const float& fTx, const float& fTy, const float& fTz)
{
	Eigen::Matrix4f Pose = CreateMatrix4f(
		Matrix3fR(0, 0), Matrix3fR(0, 1), Matrix3fR(0, 2), fTx,
		Matrix3fR(1, 0), Matrix3fR(1, 1), Matrix3fR(1, 2), fTy,
		Matrix3fR(2, 0), Matrix3fR(2, 1), Matrix3fR(2, 2), fTz);
	return Pose;
}
Eigen::Matrix3d AlgorithmVision3D::Matrix4d2Matrix3d(const Eigen::Matrix4d& Matrix4dR, double& dTx, double& dTy, double& dTz)
{
	Eigen::Matrix3d Pose = CreateMatrix3d(
		Matrix4dR(0, 0), Matrix4dR(0, 1), Matrix4dR(0, 2),
		Matrix4dR(1, 0), Matrix4dR(1, 1), Matrix4dR(1, 2),
		Matrix4dR(2, 0), Matrix4dR(2, 1), Matrix4dR(2, 2));
	dTx = Matrix4dR(0, 3);
	dTy = Matrix4dR(1, 3);
	dTz = Matrix4dR(2, 3);
	return Pose;
}
Eigen::Matrix3d AlgorithmVision3D::Matrix4d2Matrix3d(const Eigen::Matrix4d& Matrix4dR)
{
	Eigen::Matrix3d Pose = CreateMatrix3d(
		Matrix4dR(0, 0), Matrix4dR(0, 1), Matrix4dR(0, 2),
		Matrix4dR(1, 0), Matrix4dR(1, 1), Matrix4dR(1, 2),
		Matrix4dR(2, 0), Matrix4dR(2, 1), Matrix4dR(2, 2));
	return Pose;
}
Eigen::Matrix3f AlgorithmVision3D::Matrix4f2Matrix3f(const Eigen::Matrix4f& Matrix4fR)
{
	Eigen::Matrix3f Pose = CreateMatrix3f(
		Matrix4fR(0, 0), Matrix4fR(0, 1), Matrix4fR(0, 2),
		Matrix4fR(1, 0), Matrix4fR(1, 1), Matrix4fR(1, 2),
		Matrix4fR(2, 0), Matrix4fR(2, 1), Matrix4fR(2, 2));
	return Pose;
}


//显示Matrix4f矩阵
void AlgorithmVision3D::ShowMatrix4f(const Eigen::Matrix4f& Pose, const std::string& sNameLine)
{
	if (sNameLine != "")
	{
		std::cout << sNameLine << ": " << std::endl;
	}
	std::cout << Pose << std::endl;
}
//显示Matrix4d矩阵
void AlgorithmVision3D::ShowMatrix4d(const Eigen::Matrix4d& Pose, const std::string& sNameLine)
{
	if (sNameLine != "")
	{
		std::cout << sNameLine << ": " << std::endl;
	}
	std::cout << Pose << std::endl;
}

//转俯仰角形式显示Matrix4f矩阵
void AlgorithmVision3D::ShowMatrix4fonRPY(const Eigen::Matrix4f& Pose, const std::string& sNameLine)
{
	Matrix4d dPose = Matrix4f2Matrix4d(Pose);
	ShowMatrix4donRPY(dPose, sNameLine);

	//if (sNameLine != "")
	//{
	//	std::cout << sNameLine << ": ";
	//}
	////转Eigen 转 欧拉角
	//double dX, dY, dZ;//偏移量
	//double dXA, dYA, dZA;//欧拉角
	//MatrixPose2EulerPose(Pose, &dX, &dY, &dZ, &dXA, &dYA, &dZA);
	////转换欧拉角为近零的一组角度
	//eulerAngleTranMini(dXA, dYA, dZA, &dXA, &dYA, &dZA);
	//std::cout << "dX:" << dX << " dY:" << dY << " dZ:" << dZ << " dXA:" << dXA << " dYA:" << dYA << " dZA:" << dZA << std::endl;
}

//转俯仰角形式显示Matrix4d矩阵
void AlgorithmVision3D::ShowMatrix4donRPY(const Eigen::Matrix4d& Pose, const std::string& sNameLine)
{
	if (sNameLine != "")
	{
		std::cout << sNameLine << ": ";
	}
	//转Eigen 转 欧拉角
	double dX, dY, dZ;//偏移量
	double dXA, dYA, dZA;//欧拉角
	MatrixPose2EulerPose(Pose, &dX, &dY, &dZ, &dXA, &dYA, &dZA);
	//转换欧拉角为近零的一组角度
	eulerAngleTranMini(dXA, dYA, dZA, &dXA, &dYA, &dZA);
	std::cout << "dX:" << dX << " dY:" << dY << " dZ:" << dZ << " dXA:" << dXA << " dYA:" << dYA << " dZA:" << dZA << std::endl;
}


//显示Mat4d位姿矩阵
void AlgorithmVision3D::ShowHomMat3D(const cv::Mat& matMatrix4d, const std::string& sNameLine)
{
	//std::cout << matMatrix4d << std::endl;//全显示不好看
	Eigen::Matrix4d pose_matrix = Mat4d2Matrix4d(matMatrix4d);
	ShowMatrix4d(pose_matrix, sNameLine);

}
//转俯仰角形式显示Mat4d位姿矩阵
void AlgorithmVision3D::ShowHomMat3DonRPY(const cv::Mat& matMatrix4d, const std::string& sNameLine)
{
	Eigen::Matrix4d pose_matrix = Mat4d2Matrix4d(matMatrix4d);
	ShowMatrix4donRPY(pose_matrix, sNameLine);
}


//Matrix4f 转 opencv mat
cv::Mat AlgorithmVision3D::Matrix4f2Mat4d(Eigen::Matrix4f matMatrix4f)
{
	cv::Mat HomMat4d = cv::Mat::eye(4, 4, CV_64FC1);
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			HomMat4d.at<double>(i, j) = matMatrix4f(i, j);//float转double
		}
	}
	return HomMat4d;
}
//Matrix4d 转 opencv mat
cv::Mat AlgorithmVision3D::Matrix4d2Mat4d(Eigen::Matrix4d matMatrix4d)
{
	cv::Mat HomMat4d = cv::Mat::eye(4, 4, CV_64FC1);
	//if (!matMatrix4d.empty())
	{
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++)
			{
				HomMat4d.at<double>(i, j) = matMatrix4d(i, j);//float转double
			}
		}
	}
	return HomMat4d;
}

//vvd转Mat矩阵，依据vvd矩阵大小创建mat
cv::Mat AlgorithmVision3D::vvd2Matd(const std::vector< std::vector<double>>& vvdTable)
{
	//if (vvdTable.size() == 0)
	//{
	//	return cv::Mat();
	//}

	const std::vector<double>& LineData = vvdTable[0];
	//if (LineData.size() == 0)
	//{
	//	return cv::Mat();
	//}

	cv::Mat MatTable = cv::Mat::zeros(vvdTable.size(), LineData.size(), CV_64FC1);
	//if (!matMatrix4d.empty())
	{
		for (int i = 0; i < vvdTable.size(); i++)
		{
			for (int j = 0; j < LineData.size(); j++)
			{
				MatTable.at<double>(i, j) = vvdTable[i][j];
			}
		}
	}
	return MatTable;
}
cv::Mat AlgorithmVision3D::vvd2Matf(const std::vector< std::vector<double>>& vvdTable)
{
	const std::vector<double>& LineData = vvdTable[0];
	cv::Mat MatTable = cv::Mat::zeros(vvdTable.size(), LineData.size(), CV_32FC1);
	//if (!matMatrix4d.empty())
	{
		for (int i = 0; i < vvdTable.size(); i++)
		{
			for (int j = 0; j < LineData.size(); j++)
			{
				MatTable.at<float>(i, j) = vvdTable[i][j];//float转double
			}
		}
	}
	return MatTable;
}

Eigen::Matrix4f AlgorithmVision3D::vvd2Matrix4f(const std::vector< std::vector<double>>& vvdTable)
{

	Eigen::Matrix4f fMatrix = Eigen::Matrix4f::Identity();

	const std::vector<double>& LineData = vvdTable[0];
	for (int i = 0; i < vvdTable.size(); i++)
	{
		for (int j = 0; j < LineData.size(); j++)
		{
			fMatrix(i, j) = vvdTable[i][j];//float转double
		}
	}
	return fMatrix;

}
Eigen::Matrix4d AlgorithmVision3D::vvd2Matrix4d(const std::vector< std::vector<double>>& vvdTable)
{
	Eigen::Matrix4d fMatrix = Eigen::Matrix4d::Identity();

	const std::vector<double>& LineData = vvdTable[0];
	for (int i = 0; i < vvdTable.size(); i++)
	{
		for (int j = 0; j < LineData.size(); j++)
		{
			fMatrix(i, j) = vvdTable[i][j];//float转double
		}
	}
	return fMatrix;

}

std::vector< std::vector<double>> AlgorithmVision3D::Matrix4f2vvd(const Eigen::Matrix4f matrix4f)
{
	std::vector< std::vector<double>> vvdTable(4);
	for (int i = 0; i < 4; i++)
	{
		std::vector<double> vdLine(4);
		for (int j = 0; j < 4; j++)
		{
			vdLine[j] = matrix4f(i, j);
		}
		vvdTable[i] = vdLine;
	}
	return vvdTable;
}

std::vector< std::vector<double>> AlgorithmVision3D::Matrix4d2vvd(const Eigen::Matrix4d matrix4d)
{
	std::vector< std::vector<double>> vvdTable(4);
	for (int i = 0; i < 4; i++)
	{
		std::vector<double> vdLine(4);
		for (int j = 0; j < 4; j++)
		{
			vdLine[j] = matrix4d(i, j);
		}
		vvdTable[i] = vdLine;
	}
	return vvdTable;
}



//cvCloud2vPoint3f 有序点云转无序点云 0 0 0 点云为无效点云  仅转换一个 先set后转vector
std::vector<cv::Point3f> AlgorithmVision3D::cvCloud2vPoint3f(const cv::Mat& cvCloud, bool bswapXY)
{
	//std::set<cv::Point3f> sP3f;
	//std::vector<cv::Point3f> vP3f(cvCloud.rows* cvCloud.cols);
	std::vector<cv::Point3f> vP3f;
	for (int i = 0; i < cvCloud.rows; i++)
	{
		for (int j = 0; j < cvCloud.cols; j++)
		{
			cv::Vec3f Point = cvCloud.at<cv::Vec3f>(i, j);

			//cv::Point3f P = cv::Point3f(Point[0], Point[1], Point[2]);
			//sP3f.insert(P);
			if (Point[0] != 0 || Point[1] != 0 || Point[2] != 0)
			{
				if (bswapXY)
				{
					vP3f.push_back(cv::Point3f(Point[1], Point[0], Point[2]));//交换XY
				}
				else
				{
					vP3f.push_back(cv::Point3f(Point[0], Point[1], Point[2]));//正常
				}

			}
			//vP3f[i * cvCloud.cols + j] = cv::Point3f(Point[0], Point[1], Point[2]);
		}
	}

	//set<cv::Point3f> sP3f(vP3f.begin(), vP3f.end());//在构造函数中可以直接实现vector转set
	//vP3f.assign(sP3f.begin(), sP3f.end());//用assign实现set转vector

	//vP3f.assign(sP3f.begin(), sP3f.end());//用assign实现set转vector
	return vP3f;
}

//vvd转vPoint
std::vector<cv::Point3f> AlgorithmVision3D::vvd2vPoint3f(const std::vector< std::vector<double>>& vvdTable)
{
	const std::vector<double>& LineData = vvdTable[0];
	std::vector<cv::Point3f> vPoint3f(vvdTable.size());
	{
		for (int i = 0; i < vvdTable.size(); i++)
		{
			cv::Point3f Point(vvdTable[i][0], vvdTable[i][1], vvdTable[i][2]);
			vPoint3f[i] = Point;
		}
	}
	return vPoint3f;
}
std::vector<cv::Point3d> AlgorithmVision3D::vvd2vPoint3d(const std::vector< std::vector<double>>& vvdTable)
{
	const std::vector<double>& LineData = vvdTable[0];
	std::vector<cv::Point3d> vPoint3d(vvdTable.size());
	{
		for (int i = 0; i < vvdTable.size(); i++)
		{
			cv::Point3d Point(vvdTable[i][0], vvdTable[i][1], vvdTable[i][2]);
			vPoint3d[i] = Point;
		}
	}
	return vPoint3d;
}

//vPoint转vvd
std::vector< std::vector<double>> AlgorithmVision3D::vPoint2vvd(const std::vector<cv::Point2f>& vPoint)
{
	std::vector< std::vector<double>> vvdTable(vPoint.size());
	for (int i = 0; i < vPoint.size(); i++)
	{
		std::vector<double> vdPoint = { vPoint[i].x,vPoint[i].y };
		vvdTable[i] = vdPoint;
	}
	return vvdTable;
}
std::vector< std::vector<double>> AlgorithmVision3D::vPoint2vvd(const std::vector<cv::Point3f>& vPoint)
{
	std::vector< std::vector<double>> vvdTable(vPoint.size());
	for (int i = 0; i < vPoint.size(); i++)
	{
		std::vector<double> vdPoint = { vPoint[i].x,vPoint[i].y,vPoint[i].z };
		vvdTable[i] = vdPoint;
	}
	return vvdTable;
}


//Mat矩阵转vvd表
std::vector< std::vector<double>> AlgorithmVision3D::mat2vvd(const cv::Mat& dataTable)
{
	int type = dataTable.type();
	std::vector< std::vector<double>> vvdTable(dataTable.rows);
	for (int i = 0; i < dataTable.rows; i++)
	{
		std::vector<double> vdLine(dataTable.cols);
		for (int j = 0; j < dataTable.cols; j++)
		{
			//vdLine[j]=
			if (type == CV_32F)
			{
				vdLine[j] = dataTable.at<float>(i, j);
			}
			else if (type == CV_64F)
			{
				vdLine[j] = dataTable.at<double>(i, j);
			}
			else if (type == CV_8U || type == CV_8S)
			{
				vdLine[j] = dataTable.at<char>(i, j);
			}
			else if (type == CV_16U || type == CV_16S)
			{
				vdLine[j] = dataTable.at<int>(i, j);
			}
		}
		vvdTable[i] = vdLine;
	}

	return vvdTable;
}

//3D点表转mat齐次矩阵表，多一列1
cv::Mat AlgorithmVision3D::vvd3DPoint2MatfHom(const std::vector< std::vector<double>>& vvd3DPoint)
{
	const std::vector<double>& LineData = vvd3DPoint[0];
	cv::Mat MatTable = cv::Mat::ones(vvd3DPoint.size(), LineData.size() + 1, CV_64FC1);
	//if (!matMatrix4d.empty())
	{
		for (int i = 0; i < vvd3DPoint.size(); i++)
		{
			for (int j = 0; j < LineData.size(); j++)
			{
				MatTable.at<double>(i, j) = vvd3DPoint[i][j];//float转double
			}
		}
	}
	return MatTable;
}

//vvd表转置
std::vector< std::vector<double>> AlgorithmVision3D::vvdTranspose(const std::vector< std::vector<double>>& vvdTable)
{
	if (vvdTable.size() == 0)
	{
		std::vector< std::vector<double>> a;
		return a;
	}
	const std::vector<double>& LineData = vvdTable[0];
	//cv::Mat MatTable = cv::Mat::zeros(vvdTable.size(), LineData.size(), CV_32FC1
	std::vector< std::vector<double>> OutTabel(LineData.size());
	for (int i = 0; i < LineData.size(); i++)
	{
		std::vector<double> vdLine(vvdTable.size());
		for (int j = 0; j < vvdTable.size(); j++)
		{
			vdLine[j] = vvdTable[j][i];
		}
		OutTabel[i] = vdLine;
	}
	return OutTabel;
}
std::vector< std::vector<int>> AlgorithmVision3D::vviTranspose(const std::vector< std::vector<int>>& vviTable)
{
	if (vviTable.size() == 0)
	{
		std::vector< std::vector<int>> a;
		return a;
	}
	const std::vector<int>& LineData = vviTable[0];
	//cv::Mat MatTable = cv::Mat::zeros(vvdTable.size(), LineData.size(), CV_32FC1
	std::vector< std::vector<int>> OutTabel(LineData.size());
	for (int i = 0; i < LineData.size(); i++)
	{
		std::vector<int> vdLine(vviTable.size());
		for (int j = 0; j < vviTable.size(); j++)
		{
			vdLine[j] = vviTable[j][i];
		}
		OutTabel[i] = vdLine;
	}
	return OutTabel;
}

cv::Mat AlgorithmVision3D::getAffineTransform3D(const std::vector< std::vector<double>>& vvdPoints3DX, const std::vector< std::vector<double>>& vvdPoints3DY)
{
	//Mat matPX = vvd2Matf(vvdPoints3DX);
	cv::Mat matPXHom = vvd3DPoint2MatfHom(vvdPoints3DX);
	cv::Mat matA = matPXHom;
	cv::Mat matAInv;
	cv::invert(matA, matAInv, cv::DECOMP_SVD);//求逆，支持伪逆

	cv::Mat matPY = vvd2Matd(vvdPoints3DY);

	//std::vector< std::vector<double>> PoseXT = vvdTranspose(PoseX);
	//Mat matPXT = vvd2Matf(PoseXT);

	//Mat matPXrow = matPX.row(0);
	//Mat matPXcol = matPX.col(0);
	cv::Mat HomMat4d = cv::Mat::eye(4, 4, CV_64FC1);
	for (int i = 0; i < 3; i++)
	{
		cv::Mat matPYcol = matPY.col(i);
		cv::Mat abc = matAInv * matPYcol;
		cv::transpose(abc, abc);
		HomMat4d.at<double>(i, 0) = abc.at<double>(0, 0);
		HomMat4d.at<double>(i, 1) = abc.at<double>(0, 1);
		HomMat4d.at<double>(i, 2) = abc.at<double>(0, 2);
		HomMat4d.at<double>(i, 3) = abc.at<double>(0, 3);
	}
	return HomMat4d;
}

//对cvCloud点云进行仿射变换 32ms
void AlgorithmVision3D::AffineTransform3D(const cv::Mat& cvCloudIn, const cv::Mat& matAffine, cv::Mat* pcvCloudOut)
{
	cv::Mat cvCloudOut = cvCloudIn.clone();
	//500ms
	if (0)
	{//每个像素点独立变换，耗时过多 300ms
		for (int i = 0; i < cvCloudIn.rows; i++)
		{
			for (int j = 0; j < cvCloudIn.cols; j++)
			{
				cv::Vec3f Point = cvCloudIn.at<cv::Vec3f>(i, j);

				//cv::Point3f P = cv::Point3f(Point[0], Point[1], Point[2]);
				//sP3f.insert(P);
				if (Point[0] != 0 || Point[1] != 0 || Point[2] != 0)
				{
					//vP3f.push_back(cv::Point3f(Point[0], Point[1], Point[2]));
					/*Mat m1(4, 1, CV_32F, Point);*/
					cv::Mat matPointIn = (cv::Mat_<float>(4, 1) << Point[0], Point[1], Point[2], 1);//直接赋初始值的方法
					//单点变换
					cv::Mat matPointOut = matAffine * matPointIn;
					//赋值
					cv::Vec3f PointOut = { matPointOut.at<float>(0,0),matPointOut.at<float>(1,0) ,matPointOut.at<float>(2,0) };
					cvCloudOut.at<cv::Vec3f>(i, j) = PointOut;
				}
				//vP3f[i * cvCloud.cols + j] = cv::Point3f(Point[0], Point[1], Point[2]);
			}
		}
	}

	if (1)//32ms
	{//先转换、后统一变换、再转换

		//统计图像中不为0的像素点数量
		vector<bool> vbCloudPointIndex(cvCloudIn.rows * cvCloudIn.cols);
		int iPointNum = 0;
		int iPointNumIndex = 0;
		int iPixIndex = 0;
		for (int i = 0; i < cvCloudIn.rows; i++)
		{
			for (int j = 0; j < cvCloudIn.cols; j++)
			{
				cv::Vec3f Point = cvCloudIn.at<cv::Vec3f>(i, j);
				if (Point[0] != 0 || Point[1] != 0 || Point[2] != 0)
				{
					vbCloudPointIndex[iPixIndex] = true;
					iPointNumIndex++;
				}
				else {
					vbCloudPointIndex[iPixIndex] = false;
				}

				iPixIndex++;
			}
		}
		iPointNum = iPointNumIndex;
		cv::Mat cvCloudIn_nx4 = cv::Mat::ones(4, iPointNum, CV_32FC1);

		//赋值
		iPointNumIndex = 0;
		for (int i = 0; i < cvCloudIn.rows; i++)
		{
			for (int j = 0; j < cvCloudIn.cols; j++)
			{
				cv::Vec3f Point = cvCloudIn.at<cv::Vec3f>(i, j);
				if (Point[0] != 0 || Point[1] != 0 || Point[2] != 0)
				{

					cvCloudIn_nx4.at<float>(0, iPointNumIndex) = Point[0];
					cvCloudIn_nx4.at<float>(1, iPointNumIndex) = Point[1];
					cvCloudIn_nx4.at<float>(2, iPointNumIndex) = Point[2];

					iPointNumIndex++;
				}
			}
		}
		//变换
		cv::Mat cvCloudOut_nx4 = matAffine * cvCloudIn_nx4;

		//转换输出
		//Mat cvCloudOut_ = cv::Mat(cvCloudIn.rows, cvCloudIn.cols, CV_32FC1);
		//这样子变为了无序点云了~~~~  利用vbCloudPointIndex
		cv::Mat cvCloudOut_ = cv::Mat::zeros(cvCloudIn.rows, cvCloudIn.cols, CV_32FC3);
		iPointNumIndex = 0;
		iPixIndex = 0;
		for (int i = 0; i < cvCloudOut_.rows; i++)
		{
			for (int j = 0; j < cvCloudOut_.cols; j++)
			{
				if (vbCloudPointIndex[iPixIndex] == true)
				{
					cv::Vec3f PixCloud = {
							cvCloudOut_nx4.at<float>(0, iPointNumIndex),
							cvCloudOut_nx4.at<float>(1, iPointNumIndex),
							cvCloudOut_nx4.at<float>(2, iPointNumIndex) };
					iPointNumIndex++;
					cvCloudOut_.at<cv::Vec3f>(i, j) = PixCloud;
				}
				iPixIndex++;
			}
		}
		cvCloudOut = cvCloudOut_;
	}

	if (0)
	{//先转换、后统一变换、再转换
		cv::Mat cvCloudIn_nx4 = cv::Mat::ones(4, cvCloudIn.rows * cvCloudIn.cols, CV_32FC1);
		int iPointNumIndex = 0;
		for (int i = 0; i < cvCloudIn.rows; i++)
		{
			for (int j = 0; j < cvCloudIn.cols; j++)
			{
				cv::Vec3f Point = cvCloudIn.at<cv::Vec3f>(i, j);
				//if (Point[0] != 0 || Point[1] != 0 || Point[2] != 0)
				{
					cvCloudIn_nx4.at<float>(0, iPointNumIndex) = Point[0];
					cvCloudIn_nx4.at<float>(1, iPointNumIndex) = Point[1];
					cvCloudIn_nx4.at<float>(2, iPointNumIndex) = Point[2];
					iPointNumIndex++;
				}
			}
		}
		//变换
		cv::Mat cvCloudOut_nx4 = matAffine * cvCloudIn_nx4;

		//转换输出
		cv::Mat cvCloudOut_ = cv::Mat(cvCloudIn.rows, cvCloudIn.cols, CV_32FC1);
		iPointNumIndex = 0;
		for (int i = 0; i < cvCloudOut_.rows; i++)
		{
			for (int j = 0; j < cvCloudOut_.cols; j++)
			{
				cv::Vec3f PixCloud = {
				cvCloudIn_nx4.at<float>(0, iPointNumIndex),
				cvCloudIn_nx4.at<float>(1, iPointNumIndex),
				cvCloudIn_nx4.at<float>(2, iPointNumIndex) };
				iPointNumIndex++;
				cvCloudOut_.at<cv::Vec3f>(i, j) = PixCloud;
			}
		}
		//无序点置位0
		//。。。

	}


	*pcvCloudOut = cvCloudOut;

}


//变换3D点坐标
cv::Point3d AlgorithmVision3D::AffineTransPonit3d(const cv::Point3d& PointX, const cv::Mat& C1InC2)
{
	cv::Mat PointInC1 = cv::Mat::ones(4, 1, CV_64FC1);

	PointInC1.at<double>(0, 0) = PointX.x;
	PointInC1.at<double>(1, 0) = PointX.y;
	PointInC1.at<double>(2, 0) = PointX.z;
	PointInC1.at<double>(3, 0) = 1;
	cv::Mat PointInC2 = C1InC2 * PointInC1;


	cv::Point3d Point3dInC2(PointInC2.at<double>(0, 0), PointInC2.at<double>(1, 0), PointInC2.at<double>(2, 0));
	return Point3dInC2;


}
//cv::Point3d AffineTransPonit3d(const cv::Point3d PointX&, const Eigen::Matrix4d& C1InC2);

//opencv mat转Matrix4f
Eigen::Matrix4f AlgorithmVision3D::Mat4d2Matrix4f(cv::Mat matMatrix4d)
{
	Eigen::Matrix4f fMatrix = Eigen::Matrix4f::Identity();
	if (!matMatrix4d.empty())
	{
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++)
			{
				fMatrix(i, j) = matMatrix4d.at<double>(i, j);//double转float
			}
		}
	}
	return fMatrix;
}
//opencv mat转Matrix4d
Eigen::Matrix4d AlgorithmVision3D::Mat4d2Matrix4d(cv::Mat matMatrix4d)
{
	int type = matMatrix4d.type();
	Eigen::Matrix4d dMatrix = Eigen::Matrix4d::Identity();
	if (!matMatrix4d.empty())
	{
		for (int i = 0; i < 4; i++)
		{
			for (int j = 0; j < 4; j++)
			{
				//dMatrix(i, j) = matMatrix4d.at<double>(i, j);
				if (type == CV_32F)
				{
					dMatrix(i, j) = matMatrix4d.at<float>(i, j);
				}
				else if (type == CV_64F)
				{
					dMatrix(i, j) = matMatrix4d.at<double>(i, j);
				}
			}
		}
	}
	return dMatrix;
}

//位姿矩阵变换：Matrix4d转Matrix4f
Eigen::Matrix4f AlgorithmVision3D::Matrix4d2Matrix4f(Eigen::Matrix4d dMatrix)
{
	Eigen::Matrix4f fMatrix;
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			fMatrix(i, j) = (float)dMatrix(i, j);//double转float
		}
	}
	return fMatrix;
}

//位姿矩阵变换：Matrix4f转Matrix4d
Eigen::Matrix4d AlgorithmVision3D::Matrix4f2Matrix4d(Eigen::Matrix4f fMatrix)
{
	Eigen::Matrix4d dMatrix;
	for (int i = 0; i < 4; i++)
	{
		for (int j = 0; j < 4; j++)
		{
			dMatrix(i, j) = (double)fMatrix(i, j);//float转double
		}
	}
	return dMatrix;
}

//欧拉角转换为3*3旋转矩阵位姿
void AlgorithmVision3D::Euler2RotationMatrix(double dXA, double dYA, double dZA, Eigen::Matrix3d* pdRotationMatrix)
{
	//接口用度，内用弧度
	dXA = dXA / 180 * PI;
	dYA = dYA / 180 * PI;
	dZA = dZA / 180 * PI;
	//欧拉角转换为旋转矩阵
	Eigen::Vector3d eaEulerAngles(dXA, dYA, dZA);//绕Z旋转90度(Eigen角度需要取负号才和正常halcon一样)
	eaEulerAngles = -eaEulerAngles;//(Eigen角度需要取负号才和正常halcon一样)
	Eigen::Matrix3d dRotationMatrix;
	//z-y-x旋转顺序 内旋？？ 正常的
	dRotationMatrix =
		Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX());
	//z-y-x旋转顺序 外旋？？
	//dRotationMatrix =
	//	Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX()) *
	//	Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
	//	Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ());
	//std::cout << "dRotationMatrix =\n" << dRotationMatrix << endl;

	//输出
	*pdRotationMatrix = dRotationMatrix;
}

//欧拉角转换为3*3旋转矩阵位姿
Eigen::Matrix3d AlgorithmVision3D::Euler2RotationMatrix(double dXA, double dYA, double dZA)
{
	Eigen::Matrix3d dRotationMatrix;
	Euler2RotationMatrix(dXA, dYA, dZA, &dRotationMatrix);
	return dRotationMatrix;
}



//欧拉角位姿转换为4*4矩阵位姿
void AlgorithmVision3D::EulerPose2MatrixPose(double dX, double dY, double dZ, double dXA, double dYA, double dZA, Eigen::Matrix4d* pdMatrixPose)
{
	Eigen::Matrix3d dRotationMatrix = Euler2RotationMatrix(dXA, dYA, dZA);
	Eigen::Matrix4d MatrixPose = Eigen::Matrix4d::Identity();
	MatrixPose.block(0, 0, 3, 3) = dRotationMatrix;
	MatrixPose(0, 3) = dX;
	MatrixPose(1, 3) = dY;
	MatrixPose(2, 3) = dZ;
	//std::cout << "MatrixPose =\n" << MatrixPose << endl;
	*pdMatrixPose = MatrixPose;
}

//欧拉角位姿转换为4*4矩阵位姿
Eigen::Matrix4d AlgorithmVision3D::EulerPose2MatrixPose(double dX, double dY, double dZ, double dXA, double dYA, double dZA)
{
	Eigen::Matrix4d MatrixPose = Eigen::Matrix4d::Identity();
	EulerPose2MatrixPose(dX, dY, dZ, dXA, dYA, dZA, &MatrixPose);
	return MatrixPose;
}

//4*4矩阵位姿转欧拉角位姿
void AlgorithmVision3D::MatrixPose2EulerPose(Eigen::Matrix4f fMatrixPose, double* pdX, double* pdY, double* pdZ, double* pdXA, double* pdYA, double* pdZA)
{
	double dX, dY, dZ;//偏移量
	double dXA, dYA, dZA;//欧拉角

	dX = fMatrixPose(0, 3);
	dY = fMatrixPose(1, 3);
	dZ = fMatrixPose(2, 3);
	Eigen::Matrix3f fRotationMatrix = fMatrixPose.block(0, 0, 3, 3);//旋转矩阵
	//旋转矩阵转欧拉角测试（有时一样，应该是没有统一变换到一定范围内的原因）
	{
		Eigen::Vector3f eulerAngle = fRotationMatrix.eulerAngles(2, 1, 0);
		//std::cout << "eulerAngle =\n" << eulerAngle << endl;
		dXA = -eulerAngle[2] * 180 / PI;
		dYA = -eulerAngle[1] * 180 / PI;
		dZA = -eulerAngle[0] * 180 / PI;
		//std::cout << "eulerAngle转换： =\n" << dXA << "  " << dYA << "  " << dZA << endl;
	}
	//输出
	*pdX = dX;
	*pdY = dY;
	*pdZ = dZ;
	*pdXA = dXA;
	*pdYA = dYA;
	*pdZA = dZA;
}
//4*4矩阵位姿转欧拉角位姿（未测试）
void AlgorithmVision3D::MatrixPose2EulerPose(Eigen::Matrix4d dMatrixPose, double* pdX, double* pdY, double* pdZ, double* pdXA, double* pdYA, double* pdZA)
{
	double dX, dY, dZ;//偏移量
	double dXA, dYA, dZA;//欧拉角

	dX = dMatrixPose(0, 3);
	dY = dMatrixPose(1, 3);
	dZ = dMatrixPose(2, 3);
	Eigen::Matrix3d dRotationMatrix = dMatrixPose.block(0, 0, 3, 3);//旋转矩阵
	//旋转矩阵转欧拉角测试（有时一样，应该是没有统一变换到一定范围内的原因）
	{
		Eigen::Vector3d eulerAngle = dRotationMatrix.eulerAngles(2, 1, 0);
		//std::cout << "eulerAngle =\n" << eulerAngle << endl;
		dXA = -eulerAngle[2] * 180 / PI;
		dYA = -eulerAngle[1] * 180 / PI;
		dZA = -eulerAngle[0] * 180 / PI;
		//std::cout << "eulerAngle转换： =\n" << dXA << "  " << dYA << "  " << dZA << endl;
	}
	//输出
	*pdX = dX;
	*pdY = dY;
	*pdZ = dZ;
	*pdXA = dXA;
	*pdYA = dYA;
	*pdZA = dZA;
}


//将欧拉角等效转换为另一种表示
void AlgorithmVision3D::eulerAngleTransverter(double dxA, double dyA, double dzA, double* pdxA2, double* pdyA2, double* pdzA2)
{
	double dxA2, dyA2, dzA2;//另一种欧拉角
	//a b两种欧拉角加变换关系为：
	//xAa-xAb=180;  或-180
	//yAa+yAb=180
	//zAa-zAb=180
	//依据关系求解另一种欧拉角表示
	dxA2 = (dxA + 180);
	if (dxA2 >= 360) dxA2 = dxA2 - 360;
	if (dxA2 <= -360) dxA2 = dxA2 + 360;
	if (dxA2 > 180) dxA2 = dxA2 - 360;
	if (dxA2 < -180) dxA2 = dxA2 + 360;

	dyA2 = (180 - dyA);
	if (dyA2 >= 360) dyA2 = dyA2 - 360;
	if (dyA2 <= -360) dyA2 = dyA2 + 360;
	if (dyA2 > 180) dyA2 = dyA2 - 360;
	if (dyA2 < -180) dyA2 = dyA2 + 360;

	dzA2 = (dzA + 180);
	if (dzA2 > 360) dzA2 = dzA2 - 360;
	if (dzA2 <= -360) dzA2 = dzA2 + 360;
	if (dzA2 > 180) dzA2 = dzA2 - 360;
	if (dzA2 < -180) dzA2 = dzA2 + 360;

	*pdxA2 = dxA2;
	*pdyA2 = dyA2;
	*pdzA2 = dzA2;
}

//将欧拉角等效转换为绝对值近零的角度 //转换欧拉角为近零的一组角度
void AlgorithmVision3D::eulerAngleTranMini(double dxA, double dyA, double dzA, double* pdxA2, double* pdyA2, double* pdzA2)
{
	double dxA2, dyA2, dzA2;//另一种欧拉角

	eulerAngleTransverter(dxA, dyA, dzA, &dxA2, &dyA2, &dzA2);//转为另一种角度
	//欧拉角输出显示
	//cout << "eulerAngle1:" << dxA << " " << dyA << " " << dzA << endl;
	//cout << "eulerAngle2:" << dxA2 << " " << dyA2 << " " << dzA2 << endl;
	//比较绝对值和，输出和较小的一组角度
	double dSum1 = abs(dxA) + abs(dyA) + abs(dzA);
	double dSum2 = abs(dxA2) + abs(dyA2) + abs(dzA2);
	if (dSum1 <= dSum2)
	{
		*pdxA2 = dxA;
		*pdyA2 = dyA;
		*pdzA2 = dzA;
	}
	else
	{
		*pdxA2 = dxA2;
		*pdyA2 = dyA2;
		*pdzA2 = dzA2;
	}
}


//欧拉角转换为四元数 
void AlgorithmVision3D::eulerAngle2quaternion(double dxA, double dyA, double dzA, double* pdw, double* pdx, double* pdy, double* pdz)
{
	//Eigen::Vector3d eaEulerAngles(-180 / 180 * PI, 0, -180 / 180 * PI);
	////3.2 欧拉角转换为四元数,
	//Eigen::Quaterniond Quaternion;
	//Quaternion = Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitZ()) *
	//	Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
	//	Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitX());
	//cout << "Quaternion x: " << Quaternion.x() << endl;
	//cout << "Quaternion y: " << Quaternion.y() << endl;
	//cout << "Quaternion z: " << Quaternion.z() << endl;
	//cout << "Quaternion w: " << Quaternion.w() << endl;

	Eigen::Vector3d eaEulerAngles(dxA / 180 * PI, dyA / 180 * PI, dzA / 180 * PI);
	//3.2 欧拉角转换为四元数,
	Eigen::Quaterniond Quaternion;
	Quaternion = Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX());

	//Quaternion = Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX()) *
	//	Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
	//	Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ());
	//cout << "Quaternion x: " << Quaternion.x() << endl;
	//cout << "Quaternion y: " << Quaternion.y() << endl;
	//cout << "Quaternion z: " << Quaternion.z() << endl;
	//cout << "Quaternion w: " << Quaternion.w() << endl;

	*pdw = Quaternion.w();
	*pdx = Quaternion.x();
	*pdy = Quaternion.y();
	*pdz = Quaternion.w();

}

//欧拉角位姿转换为四元数位姿 v6 ->v7
std::vector<float>  AlgorithmVision3D::eulerPose2quaternionPose(std::vector<float> PoseEuler)
{
	std::vector<float> Pose(7);
	Pose[0] = PoseEuler[0];
	Pose[1] = PoseEuler[1];
	Pose[2] = PoseEuler[2];
	double w, x, y, z;
	eulerAngle2quaternion(PoseEuler[3], PoseEuler[4], PoseEuler[5], &w, &x, &y, &z);
	Pose[3] = w;
	Pose[4] = x;
	Pose[5] = y;
	Pose[6] = z;
	return Pose;

}

//欧拉角转换为四元数 
void AlgorithmVision3D::eulerAngle2quaternion2(double dxA, double dyA, double dzA, double* pdw, double* pdx, double* pdy, double* pdz)
{
	//Eigen::Vector3d eaEulerAngles(-180 / 180 * PI, 0, -180 / 180 * PI);
	////3.2 欧拉角转换为四元数,
	//Eigen::Quaterniond Quaternion;
	//Quaternion = Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitZ()) *
	//	Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
	//	Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitX());
	//cout << "Quaternion x: " << Quaternion.x() << endl;
	//cout << "Quaternion y: " << Quaternion.y() << endl;
	//cout << "Quaternion z: " << Quaternion.z() << endl;
	//cout << "Quaternion w: " << Quaternion.w() << endl;

	Eigen::Vector3d eaEulerAngles(dxA / 180 * PI, dyA / 180 * PI, dzA / 180 * PI);
	//3.2 欧拉角转换为四元数,
	Eigen::Quaterniond Quaternion;
	Quaternion = Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX()) *
		Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ());

	//Quaternion = Eigen::AngleAxisd(eaEulerAngles[0], Eigen::Vector3d::UnitX()) *
	//	Eigen::AngleAxisd(eaEulerAngles[1], Eigen::Vector3d::UnitY()) *
	//	Eigen::AngleAxisd(eaEulerAngles[2], Eigen::Vector3d::UnitZ());
	//cout << "Quaternion x: " << Quaternion.x() << endl;
	//cout << "Quaternion y: " << Quaternion.y() << endl;
	//cout << "Quaternion z: " << Quaternion.z() << endl;
	//cout << "Quaternion w: " << Quaternion.w() << endl;

	*pdw = Quaternion.w();
	*pdx = Quaternion.x();
	*pdy = Quaternion.y();
	*pdz = Quaternion.w();

}

//欧拉角位姿转换为四元数位姿 v6 ->v7
std::vector<float> AlgorithmVision3D::eulerPose2quaternionPose2(std::vector<float> PoseEuler)
{
	std::vector<float> Pose(7);
	Pose[0] = PoseEuler[0];
	Pose[1] = PoseEuler[1];
	Pose[2] = PoseEuler[2];
	double w, x, y, z;
	eulerAngle2quaternion2(PoseEuler[3], PoseEuler[4], PoseEuler[5], &w, &x, &y, &z);
	Pose[3] = w;
	Pose[4] = x;
	Pose[5] = y;
	Pose[6] = z;
	return Pose;

}

inline Eigen::Matrix3d AlgorithmVision3D::Matrix3f2Matrix3d(const Eigen::Matrix3f& Matrix3fR)
{
	Eigen::Matrix3d dMatrix = Eigen::Matrix3d::Identity();
	dMatrix(0, 0) = Matrix3fR(0, 0); dMatrix(0, 1) = Matrix3fR(0, 1); dMatrix(0, 2) = Matrix3fR(0, 2);
	dMatrix(1, 0) = Matrix3fR(1, 0); dMatrix(1, 1) = Matrix3fR(1, 1); dMatrix(1, 2) = Matrix3fR(1, 2);
	dMatrix(2, 0) = Matrix3fR(2, 0); dMatrix(2, 1) = Matrix3fR(2, 1); dMatrix(2, 2) = Matrix3fR(2, 2);
	return dMatrix;
}

inline Eigen::Matrix3f AlgorithmVision3D::Matrix3d2Matrix3f(const Eigen::Matrix3d& Matrix3dR)
{
	Eigen::Matrix3f dMatrix = Eigen::Matrix3f::Identity();
	dMatrix(0, 0) = Matrix3dR(0, 0); dMatrix(0, 1) = Matrix3dR(0, 1); dMatrix(0, 2) = Matrix3dR(0, 2);
	dMatrix(1, 0) = Matrix3dR(1, 0); dMatrix(1, 1) = Matrix3dR(1, 1); dMatrix(1, 2) = Matrix3dR(1, 2);
	dMatrix(2, 0) = Matrix3dR(2, 0); dMatrix(2, 1) = Matrix3dR(2, 1); dMatrix(2, 2) = Matrix3dR(2, 2);
	return dMatrix;
}
inline Eigen::Affine3d AlgorithmVision3D::Matrix4d2Affine3d(const Eigen::Matrix4d& matrix4d)
{
	return Eigen::Affine3d(matrix4d);
}
inline Eigen::Affine3f AlgorithmVision3D::Matrix4f2Affine3f(const Eigen::Matrix4f& matrix4f)
{
	Eigen::Transform<float, 3, Eigen::Affine> a3f_transform(matrix4f);//标准转换？？
	return a3f_transform;
}
